#include "app_motion_control.h"

// 设备驱动调用
#include "device.h"
extern device_t device;
static motor_t *motor_pwm = &device.motor;

// 算法库调用
#include "algorithm.h"
extern algo_imu_t algo_imu;				// 惯性测量单元算法
extern algo_pid_t algo_pid_pitch_angle;	// 飞行器俯仰角度PID
extern algo_pid_t algo_pid_roll_angle;	// 飞行器横滚角度PID
extern algo_pid_t algo_pid_yaw_angle;	// 飞行器偏航角度PID
extern algo_pid_t algo_pid_pitch_gyro; 	// 飞行器俯仰角速度PID
extern algo_pid_t algo_pid_roll_gyro;	// 飞行器横滚角速度PID
extern algo_pid_t algo_pid_yaw_gyro;	// 飞行器偏航角速度PID

// 变量初始化
motion_control_t g_motion_control =
{
	.state = STOP,
	
	.posture_control.expc_pitch_angle = 0,
	.posture_control.expc_roll_angle = 0,
	.posture_control.expc_yaw_angle = 0,
	.posture_control.cur_pitch_angle = 0,
	.posture_control.cur_roll_angle = 0,
	.posture_control.cur_yaw_angle = 0,
	
	.posture_control.expc_pitch_gyro = 0,
	.posture_control.expc_roll_gyro = 0,
	.posture_control.expc_yaw_gyro = 0,
	.posture_control.cur_pitch_gyro = 0,
	.posture_control.cur_roll_gyro = 0,
	.posture_control.cur_yaw_gyro = 0,
	
	.motor_control.motor_total[0] = 0,
	.motor_control.motor_total[1] = 0,
	.motor_control.motor_total[2] = 0,
	.motor_control.motor_total[3] = 0,
	.motor_control.motor_pwm_basic = 850,
	.motor_control.motor_pwm_limit = 990,
};


// 自动飞行启动倒计时（此函数用于定时器中断中）
// tim_freq：定时器工作频率
// time：按键长按时间（单位：秒）
static void auto_ctrl_countdown(uint32 tim_freq, f32 time)
{
	static uint32 t = 0;
	const uint32 time_cnt = (uint32)(tim_freq * time);
	
	if (g_motion_control.state == WAIT)
	{
		++t;
		if (t >= time_cnt)
		{
			t = 0;
			g_motion_control.state = AUTO_CTRL;
		}
	}
	else
	{
		t = 0;
	}
}

// 获取飞行器期望姿态
static void expc_posture_get(void)
{
	// 获取当前飞行器姿态角速度
	g_motion_control.posture_control.cur_pitch_gyro = (int16)algo_imu.pitch.gyro;
	g_motion_control.posture_control.cur_roll_gyro = (int16)algo_imu.roll.gyro;
	g_motion_control.posture_control.cur_yaw_gyro = (int16)algo_imu.yaw.gyro;
	
	// 获取当前飞行器姿态角度
	g_motion_control.posture_control.cur_pitch_angle = (int16)algo_imu.pitch.angle;
	g_motion_control.posture_control.cur_roll_angle = (int16)algo_imu.roll.angle;
	g_motion_control.posture_control.cur_yaw_angle = (int16)algo_imu.yaw.angle;
	
	// 获取期望飞行器姿态角度
	g_motion_control.posture_control.expc_pitch_angle = 0;
	g_motion_control.posture_control.expc_roll_angle = 0;
	g_motion_control.posture_control.expc_yaw_angle = 0;
}

// 限制PWM输出
static void motor_pwm_limit(int16 *pwm, int16 upper_limit, int16 lower_limit)
{
	if (*pwm > upper_limit) 		*pwm = upper_limit;
	else if (*pwm < lower_limit) 	*pwm = lower_limit;
}

// 计算电机PWM输出值
static void motor_pwmout_calc(void)
{
	int16 motor_posture_pwm[4], motor_navigation_pwm[4];
	int16 pitch_posture_pwm = 0, roll_posture_pwm = 0, yaw_posture_pwm = 0;
	
	memset(motor_posture_pwm, 0, sizeof(motor_posture_pwm));		// 姿态PWM
	memset(motor_navigation_pwm, 0, sizeof(motor_navigation_pwm));	// 导航PWM
	
	// 计算导航输出值
	motor_navigation_pwm[0] = g_motion_control.motor_control.motor_pwm_basic;
	motor_navigation_pwm[1] = g_motion_control.motor_control.motor_pwm_basic;
	motor_navigation_pwm[2] = g_motion_control.motor_control.motor_pwm_basic;
	motor_navigation_pwm[3] = g_motion_control.motor_control.motor_pwm_basic;
	
	// 计算姿态输出值
	// 外环角度PID
	g_motion_control.posture_control.expc_pitch_gyro = algo_pid_calc(&algo_pid_pitch_angle, (f32)g_motion_control.posture_control.expc_pitch_angle, (f32)g_motion_control.posture_control.cur_pitch_angle);
	g_motion_control.posture_control.expc_roll_gyro = algo_pid_calc(&algo_pid_roll_angle, (f32)g_motion_control.posture_control.expc_roll_angle, (f32)g_motion_control.posture_control.cur_roll_angle);
	g_motion_control.posture_control.expc_yaw_gyro = algo_pid_calc(&algo_pid_yaw_angle, (f32)g_motion_control.posture_control.expc_yaw_angle, (f32)g_motion_control.posture_control.cur_yaw_angle);
	
	// 内环角速度PID
	pitch_posture_pwm = algo_pid_calc(&algo_pid_pitch_gyro, (f32)g_motion_control.posture_control.expc_pitch_gyro, (f32)g_motion_control.posture_control.cur_pitch_gyro);
	roll_posture_pwm = algo_pid_calc(&algo_pid_roll_gyro, (f32)g_motion_control.posture_control.expc_roll_gyro, (f32)g_motion_control.posture_control.cur_roll_gyro);
	yaw_posture_pwm = algo_pid_calc(&algo_pid_yaw_gyro, (f32)g_motion_control.posture_control.expc_yaw_gyro, (f32)g_motion_control.posture_control.cur_yaw_gyro);
	
	// 各电机姿态输出
	motor_posture_pwm[0] = pitch_posture_pwm + roll_posture_pwm + yaw_posture_pwm;
	motor_posture_pwm[1] = pitch_posture_pwm - roll_posture_pwm - yaw_posture_pwm;
	motor_posture_pwm[2] = -pitch_posture_pwm - roll_posture_pwm + yaw_posture_pwm;
	motor_posture_pwm[3] = -pitch_posture_pwm + roll_posture_pwm - yaw_posture_pwm;
	
	// 姿态PWM输出限制
	motor_pwm_limit(&motor_posture_pwm[0], 800, -800);
	motor_pwm_limit(&motor_posture_pwm[1], 800, -800);
	motor_pwm_limit(&motor_posture_pwm[2], 800, -800);
	motor_pwm_limit(&motor_posture_pwm[3], 800, -800);
	
	
	// 大幅度偏转停机
	if ((ABS(algo_imu.pitch.angle) > 50) || (ABS(algo_imu.roll.angle) > 50)) 
	{
		g_motion_control.state = STOP;
	}
	
	// 电机PWM计算
	switch (g_motion_control.state)
	{
		case STOP:
		case WAIT:
			memset(g_motion_control.motor_control.motor_total, 0, sizeof(g_motion_control.motor_control.motor_total));
			break;
		
		case AUTO_CTRL:
		case REMOTE_CTRL:
			g_motion_control.motor_control.motor_total[0] = motor_navigation_pwm[0] + motor_posture_pwm[0];
			g_motion_control.motor_control.motor_total[1] = motor_navigation_pwm[1] + motor_posture_pwm[1];
			g_motion_control.motor_control.motor_total[2] = motor_navigation_pwm[2] + motor_posture_pwm[2];
			g_motion_control.motor_control.motor_total[3] = motor_navigation_pwm[3] + motor_posture_pwm[3];
			motor_pwm_limit(&g_motion_control.motor_control.motor_total[0], g_motion_control.motor_control.motor_pwm_limit, 0);
			motor_pwm_limit(&g_motion_control.motor_control.motor_total[1], g_motion_control.motor_control.motor_pwm_limit, 0);
			motor_pwm_limit(&g_motion_control.motor_control.motor_total[2], g_motion_control.motor_control.motor_pwm_limit, 0);
			motor_pwm_limit(&g_motion_control.motor_control.motor_total[3], g_motion_control.motor_control.motor_pwm_limit, 0);
			break;
	}
}

// 运动控制（此函数用于定时器中断中）
// timer_frequency: 定时器工作频率
void motion_control(uint32 timer_frequency)
{
	// 飞行器数据获取及处理
	imu_data_proc(&algo_imu); // 惯性测量单元数据处理，计算出偏航角等角度值与角速度值
	
	// 获取飞行器期望运动
	auto_ctrl_countdown(timer_frequency, 5); // 自动飞行5秒启动倒计时
	expc_posture_get(); // 获取飞行器期望姿态
	
	// 计算各电机输出值
	motor_pwmout_calc();
	
	// 各电机PWM输出
	motor_pwm->output_4ch(g_motion_control.motor_control.motor_total[0], g_motion_control.motor_control.motor_total[1], 
						g_motion_control.motor_control.motor_total[2], g_motion_control.motor_control.motor_total[3]);
}


